using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet
{
	public class SimulationIslandManager
	{
		private UnionFind _unionFind = new UnionFind();

		public void InitUnionFind(int n)
		{
			_unionFind.Reset(n);
		}

		public UnionFind UnionFind { get { return _unionFind; } }

		public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher)
		{
			InitUnionFind(world.CollisionObjectsCount);

			// put the index into m_controllers into m_tag	
			int index = 0;
			foreach (CollisionObject collisionObject in world.CollisionObjects)
			{
				collisionObject.IslandTag = index;
				collisionObject.HitFraction = 1;
				index++;
			}
			// do the union find
			FindUnions(dispatcher);
		}

		public virtual void StoreIslandActivationState(CollisionWorld world)
		{
			// put the islandId ('find' value) into m_tag			
			int index = 0;
			foreach (CollisionObject collisionObject in world.CollisionObjects)
			{
				if (collisionObject.MergesSimulationIslands)
				{
					collisionObject.IslandTag = _unionFind.Find(index);
				}
				else
				{
					collisionObject.IslandTag = -1;
				}
				index++;
			}
		}

		public void FindUnions(IDispatcher dispatcher)
		{
			for (int i = 0; i < dispatcher.ManifoldCount; i++)
			{
				PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
				//static objects (invmass 0.f) don't merge !

				CollisionObject colObjA = manifold.BodyA as CollisionObject;
				CollisionObject colObjB = manifold.BodyB as CollisionObject;

				if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&
					((colObjB != null) && (colObjB.MergesSimulationIslands)))
				{
					_unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);
				}
			}
		}

		public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IslandCallback callback)
		{
			//we are going to sort the unionfind array, and store the element id in the size
			//afterwards, we clean unionfind, to make sure no-one uses it anymore
			UnionFind.SortIslands();
			int numElem = UnionFind.ElementCount;

			int endIslandIndex = 1;

			//update the sleeping state for bodies, if all are sleeping
			for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
			{
				int islandId = UnionFind[startIslandIndex].Id;
				for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].Id == islandId); endIslandIndex++)
				{
				}

				//int numSleeping = 0;

				bool allSleeping = true;

				int idx;
				for (idx = startIslandIndex; idx < endIslandIndex; idx++)
				{
					int i = UnionFind[idx].Size;

					CollisionObject colObjA = collisionObjects[i];
					if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
					{
						Console.WriteLine("error in island management");
					}

					BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
					if (colObjA.IslandTag == islandId)
					{
						if (colObjA.ActivationState == ActivationState.Active)
						{
							allSleeping = false;
						}
						if (colObjA.ActivationState == ActivationState.DisableDeactivation)
						{
							allSleeping = false;
						}
					}
				}


				if (allSleeping)
				{
					for (idx = startIslandIndex; idx < endIslandIndex; idx++)
					{
						int i = UnionFind[idx].Size;
						CollisionObject colObjA = collisionObjects[i];
						if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
						{
							Console.WriteLine("error in island management");
						}

						BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));

						if (colObjA.IslandTag == islandId)
						{
							colObjA.ActivationState =ActivationState.IslandSleeping;
						}
					}
				}
				else
				{
					for (idx = startIslandIndex; idx < endIslandIndex; idx++)
					{
						int i = UnionFind[idx].Size;

						CollisionObject colObjA = collisionObjects[i];
						if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
						{
							Console.WriteLine("error in island management");
						}

						BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));

						if (colObjA.IslandTag == islandId)
						{
							if (colObjA.ActivationState == ActivationState.IslandSleeping)
							{
								colObjA.ActivationState = ActivationState.WantsDeactivation;
							}
						}
					}
				}
			}

			List<PersistentManifold> islandmanifold;
			int maxNumManifolds = dispatcher.ManifoldCount;
			islandmanifold= new List<PersistentManifold>(maxNumManifolds);

			for (int i = 0; i < maxNumManifolds && i < dispatcher.ManifoldCount; i++)
			{
				PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);

				CollisionObject colObjA = manifold.BodyA as CollisionObject;
				CollisionObject colObjB = manifold.BodyB as CollisionObject;

				//todo: check sleeping conditions!
				if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
				   ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
				{

					//kinematic objects don't merge islands, but wake up all connected objects
					if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
					{
						colObjB.Activate();
					}
					if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
					{
						colObjA.Activate();
					}

					//filtering for response
					if (dispatcher.NeedsResponse(colObjA, colObjB))
						islandmanifold.Add(manifold);
				}
			}

			int numManifolds = islandmanifold.Count;

			// Sort manifolds, based on islands
			// Sort the vector using predicate and std::sort
			islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));

			//now process all active islands (sets of manifolds for now)
			int startManifoldIndex = 0;
			int endManifoldIndex = 1;

			for (startManifoldIndex = 0; startManifoldIndex < numManifolds; startManifoldIndex = endManifoldIndex)
			{
				int islandId = GetIslandId(islandmanifold[startManifoldIndex]);
				for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++)
				{
				}
				/// Process the actual simulation, only if not sleeping/deactivated
				int numIslandManifolds = endManifoldIndex - startManifoldIndex;
				if (numIslandManifolds != 0)
				{
					callback.ProcessIsland(islandmanifold, startManifoldIndex, numIslandManifolds);
				}
			}
		}

		private int GetIslandId(PersistentManifold lhs)
		{
			int islandId;
			CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
			CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
			islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
			return islandId;
		}

		private int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
		{
			int rIslandIdA, lIslandIdB;
			rIslandIdA = GetIslandId(rhs);
			lIslandIdB = GetIslandId(lhs);
			//return lIslandId0 < rIslandId0;
			if (lIslandIdB < rIslandIdA)
				return -1;
			else if (lIslandIdB > rIslandIdA)
				return 1;
			return 0;
		}

		public abstract class IslandCallback
		{
			public abstract void ProcessIsland(List<PersistentManifold> manifolds, int startManifold, int numManifolds);
		}
	}
}
